﻿using System;
using System.Collections.Generic;
using System.Text;
using RobotRace.engine.raceengine;
using RobotRace.engine.robossConnector;
using FIRADriverLibrary;
using Geometry;
using RobotRace.Geometry;
using RobotRace.engine.path;
using RobotRace.utils.graph;

namespace RobotRace.engine.agents.implementation
{
    public class SimplePointFollower : AbstractAgent
    {
        public override String Name { get; set; }
        public override List<FIRARobotState> ControlledRobots { get { return new List<FIRARobotState> { robot }; } }
        public int CurrentCheckpoint { get; set; }
        public double MaxVelocity { get; set; }
        public double AcceptableDistanceFromCheckpoint { get; set; }

        private FIRARobotState robot;
        //private List<Point2D> pathToFollow;
        private RaceEngine engine;
        private RobossConnector connector;
        private TimeSpan lastLap;
        private DateTime lastLapStartTime;
        private Boolean validLap = false;

        // parameter stating how much speed can change in one step
        // given in fraction of the max speed
        //private double velocityChangeInertiaParameter = 0.25;
        private Point2D robotStartingPoint;

        public SimplePointFollower(Point2D robotStartingPoint, FIRARobotState controlledRobot, List<Point2D> pathToFollow) : base(pathToFollow)
        {
            this.robot = controlledRobot;
            //this.pathToFollow = pathToFollow;
            this.robotStartingPoint = robotStartingPoint;
            this.MaxVelocity = 1.0;
            this.CurrentCheckpoint = 0;
            this.AcceptableDistanceFromCheckpoint = 0.1;
        }

        public override void configure(RaceEngine engine, RobossConnector connector)
        {
            this.engine = engine;
            this.connector = connector;
            
        }

        public override void step()
        {
            if (path == null || path.Count <= 0)
            {
                return;
            }

            if (CurrentCheckpoint >= path.Count)
            {
                CurrentCheckpoint = 0;
            }

            if (!robot.owned)
            {
                throw new Exception("ROBOT NOT OWNED!");
            }

            double currentRobotXPosition = robot.x;
            double currentRobotYPosition = robot.y;
            double currentRobotAngle = robot.angle;
            double currentRobotLeftSpeed = robot.leftSpeed;
            double currentRobotRightSpeed = robot.rightSpeed;

            Node currentNode = checkpointManagementStrategy.getCurrentCheckpoint(path);

            double distanceFromNextCheckpoint = currentNode.point.GetDistance(currentRobotXPosition, currentRobotYPosition);
            
            while (robotReachedCheckpoint(distanceFromNextCheckpoint))
            {
                CurrentCheckpoint++;
                if (CurrentCheckpoint >= path.Count)
                {
                    DateTime stopTime = DateTime.Now;
                    CurrentCheckpoint = 0;
                    if (validLap == true)
                    {
                        lastLap = stopTime - lastLapStartTime;
                        engine.registerLapTime(lastLap);
                    }
                    validLap = true;
                    lastLapStartTime = DateTime.Now;
                }
                currentNode = checkpointManagementStrategy.findNextPoint(path, new Point2D(robot.x, robot.y));
                distanceFromNextCheckpoint = currentNode.point.GetDistance(currentRobotXPosition, currentRobotYPosition);
            }

            Vector2D distanceVector = new Vector2D(new Point2D(currentRobotXPosition, currentRobotYPosition), currentNode.point);
            Vector2D robotMoveVector = new Vector2D(currentRobotAngle) * (1 + (currentRobotLeftSpeed + currentRobotRightSpeed) / 2);

            double differenceAngle = Vector2D.AngleBetween(robotMoveVector, distanceVector);
            Console.WriteLine(distanceVector + " ," + robotMoveVector + ", " + differenceAngle + ", left: " + currentRobotLeftSpeed + ", right: " + currentRobotRightSpeed);
            if (differenceAngle > -Math.PI && differenceAngle <= -Math.PI / 2)
            {
                robot.rightSpeed = changeSpeed(currentRobotRightSpeed, -MaxVelocity);
                robot.leftSpeed = changeSpeed(currentRobotLeftSpeed, -MaxVelocity * adjust(1 - (Math.PI + differenceAngle)));
            }
            else if (differenceAngle <= 0)
            {
                robot.rightSpeed = changeSpeed(currentRobotRightSpeed, MaxVelocity);
                robot.leftSpeed = changeSpeed(currentRobotLeftSpeed, MaxVelocity * adjust(Math.PI / 2 + differenceAngle));
            }
            else if (differenceAngle <= Math.PI / 2)
            {
                robot.leftSpeed = changeSpeed(currentRobotLeftSpeed, MaxVelocity);
                robot.rightSpeed = changeSpeed(currentRobotRightSpeed, MaxVelocity * adjust(Math.PI / 2 - differenceAngle));
            }
            else
            {
                robot.leftSpeed = changeSpeed(currentRobotLeftSpeed, -MaxVelocity);
                robot.rightSpeed = changeSpeed(currentRobotRightSpeed, -MaxVelocity * adjust(1 - (Math.PI - differenceAngle)));
            }

        }

        public override void pause()
        {
            robot.leftSpeed = 0;
            robot.rightSpeed = 0;
        }

        public override void stop()
        {
            CurrentCheckpoint = 0;
            robot.leftSpeed = 0;
            robot.rightSpeed = 0;
            validLap = false;
        }

        private bool robotReachedCheckpoint(double distanceFromNextCheckpoint)
        {
            if (distanceFromNextCheckpoint <= AcceptableDistanceFromCheckpoint)
            {
                return true;
            }
            return false;
        }

        private Double adjust(Double d)
        {
            return Math.Abs(d / Math.PI * 2);
        }

        private double changeSpeed(double previousSpeed, double destinationSpeed)
        {
            /*double difference = Math.Abs(previousSpeed - destinationSpeed);
            double maxSpeedChange = maxVelocity * velocityChangeInertiaParameter;
            if (difference < maxSpeedChange)
            {
                return destinationSpeed;
            }
            double sign = previousSpeed > destinationSpeed ? -1.0 : 1.0;
            return previousSpeed + sign * maxSpeedChange;*/
            return destinationSpeed;
        }
    }
}
